#ifndef _FORMATION_H
#define _FORMATION_H

#include <gmm_voxel_map/gmm_voxel_map.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point32.h>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/random_sample.h>
#include <iostream>
#include <string>
#include <cmath>
#include <stdlib.h>
#include <mavros_msgs/State.h>
#include <nav_msgs/Odometry.h>
#include <control_msgs/JointControllerState.h>
#include <std_msgs/Float64.h>
#include <queue>
#include <cstdint>
#include <trajectory_msgs/JointTrajectory.h>
#include <std_msgs/ColorRGBA.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl/filters/passthrough.h>
#include <nav_msgs/Path.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <algorithm>
#include <functional>
#include "buaa/utils.h"
#include "mavros_msgs/State.h"
#include <ros/package.h>


#define PIx2 6.28318
#define PI 3.14159
#define PI_2 1.5708
#define AGENT_NUM 4
using namespace std;

class GridMapNode;

typedef GridMapNode *GridMapNodePtr;


class GridCube {
public:
    GridCube() {}

    ~GridCube() {}

    //Eigen::Vector3d p1, p2, p3, p4, p5, p6, p7, p8;   // the 8 vertex of a cube
    Eigen::MatrixXf vertex_pos;
    Eigen::Vector3i center_idx; // the center of the cube
    bool valid;    // indicates whether this cube should be deleted
    Eigen::MatrixXi vertex_idx;

    double t; // time allocated to this cube
    std::vector<std::pair<double, double> > box;

    // create a cube using 8 vertex and the center point

    GridCube(Eigen::Vector3i node_index) {
        vertex_idx = Eigen::MatrixXi::Zero(8, 3);
        for (int i = 0; i < 8; i++) {
            vertex_idx.row(i) = node_index;
        }
        vertex_pos = Eigen::MatrixXf::Zero(8, 3);
        center_idx = node_index;
    }

    bool InflateCube(Eigen::MatrixXi last_InflateCube);

    bool VertexIsContained(Eigen::MatrixXi last_InflateCube);

    void UpdateVertexPos();


};


class GridMapNode {
public:
    enum node_state {
        OPENSET = 1,
        CLOSEDSET = 2,
        UNDEFINED = 3
    };
    enum node_state state;

    GridMapNode() {}

    GridMapNode(Eigen::Vector3i node_index) : index(node_index), g_score(0), h_score(0), f_score(0), state(UNDEFINED) {}

    ~GridMapNode() {}

    double g_score;
    double h_score;
    double f_score;
    Eigen::Vector3i index;
    int rounds{0};

    GridMapNode *parent_node;


    std::vector<GridMapNode> getbresenham3D(GridMapNode *bresenham3Dend_ptr);

    int tmp;

};

class NodeComparator {
public:
    bool operator()(GridMapNode *node1, GridMapNode *node2) {
        return node1->f_score > node2->f_score;
    }
};

class Formation_Agent {

    enum FSM_EXEC_STATE {
        INIT,
        TAKEOFF,
        WAIT_TARGET,
        GEN_NEW_TRAJ,
        REPLAN_TRAJ,
        EXEC_TRAJ,
        EMERGENCY_STOP,
        SEQUENTIAL_START,
        END_TRAJ
    };


public:
    Formation_Agent(int id, double map_resolution, int map_N) : AGENT_ID(id), COLOR_ID(id + 32), AgentInitFlag(false),
                                                                gvmcb(map_resolution, 1.5, {7, 7, 6}, {0.4, 0.5, 0.5}) {
        ROS_INFO("----------- Formation_Agent init -----------");
    }

    ~Formation_Agent() {}


    gvm::GMMVoxelMapCircularBuffer gvmcb;

    int AGENT_ID;
    int COLOR_ID;
    ros::Subscriber cloud_sub;
    ros::Subscriber pose_sub;

    Eigen::Vector3d current_p;
    Eigen::Quaterniond current_q;
    std::queue<geometry_msgs::PoseStamped> pose_queue;
    double init_head_yaw;
    std::vector<geometry_msgs::PoseStamped> motor_yaw_queue;
    double CLOUD_DELAY;

    double D435_T265_OFFSET_X;
    double D435_T265_OFFSET_Z;

    double D435_BASE_OFFSET_X;
    double D435_BASE_OFFSET_Z;

    double T265_OFFSET_X;
    double T265_OFFSET_Y;
    double T265_OFFSET_Z;

    double TAKEOFF_HEIGHT;

    double CLIP_DISTANCE;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clip;

    ros::Publisher gvmcb_map_pub;
    ros::Publisher cloud_edf_pub;

    ros::Publisher current_goal_position_visual_pub;
    ros::Publisher _corridor_vis_pub;

    ros::Publisher optimize_traj_visual_pub;
    ros::Publisher setpoint_pub;
    ros::Publisher cloud_vis_pub;

    std::priority_queue<GridMapNode *, std::vector<GridMapNode *>, NodeComparator> openSet_;
    std::vector<Eigen::Vector3i> a_star_gridpath;

    const double tie_breaker_ = 1 + 1.0 / 10000;
    ros::Publisher a_star_pub;

    ros::Timer frontplantimer;
    ros::Timer exec_timer_;
    ros::Timer sendpva_timer;
    ros::Timer map_visual_timer_;

    ros::Publisher optimize_traj_pub;
    ros::Publisher plan_head_pub;
    ros::Subscriber gimbal_angle_sub;
    ros::Publisher pva_pub;

    ros::Subscriber state_sub;
    ros::Subscriber global_goal_sub;

    void init(ros::NodeHandle &nh);

    void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud);

    void pose_store(int index_temp, const geometry_msgs::PoseStampedPtr &msg);

    void poseCallback(const geometry_msgs::PoseStampedPtr &msg);

    void map_visualization();

    void a_star_RemoveCollinearPath();


    double getDiagHeu(GridMapNode *node1, GridMapNode *node2);

    double getHeu(GridMapNode *node1, GridMapNode *node2);

    bool a_star_search(Eigen::Vector3d start_p, Eigen::Vector3d end_p);

    void a_star_retrievePath(GridMapNode *final_node);

    void visual_a_star_path();

    double getpointdistance(Eigen::Vector3i idx1, Eigen::Vector3i idx2);

    void a_star_bresenham3D();

    void initGridMap();

    bool traj_optimization();

    void execFSMCallback(const ros::TimerEvent &e);

    void changeState(FSM_EXEC_STATE next_state);

    void sendPVA(const ros::TimerEvent &e);


    void gimbal_yaw_angleCallback(geometry_msgs::PoseStamped msg);

    void angle_mapping(double &angle);


    void uavmodecallback(const mavros_msgs::State &msg);

    void GlobalGoalCallback(const geometry_msgs::PoseStampedConstPtr &msg);

    void MapVisualCallback(const ros::TimerEvent &e);

    void save_traj_to_file();


    int rounds_{0};
    int pool_size{130};
    int pool_size_z{10};

    double SAFETY_DISTANCE{0.3};
    std::vector<GridCube> CorridorCube;


    GridMapNodePtr ***GridMap_;
    Eigen::Vector3d plan_end_p;
    double PlanFreq{1.0};

    double IGSector[100][2];
    double hfov{1.047};

    double SectorAngle{hfov / 4.0};

    double MaxCameraDis{5};

    double hmax = MaxCameraDis * tan(hfov / 2) * 2;
    double vmax = hmax / 640.0 * 480.0;


    int distribute_flag;


    double K_IG = -0.1;
    double K_CG = 1.0;
    double K_IDEAL = 0.5;

    double rate{30.0};

    std::vector<Eigen::Vector3d> OptimizedTraj;

    FSM_EXEC_STATE exec_state_;
    Eigen::Vector3d local_goal;
    Eigen::Vector3d global_goal;
    Eigen::Vector3d takeoff_pose;


    double v = 0.7;
public:
    bool AgentInitFlag;

    double local_length{1.8};
    float LAND_HEIGHT = 0.0;

    trajectory_msgs::JointTrajectory traj;

    int sendpva_count = 0;

    bool offboard_ready = false;

    bool a_star_flag = false;

    Eigen::Vector3d stop_p;

    bool first_fail = true;
    bool FLAG_TAKEOFF_DONE = false;
    int COUNT_TAKEOFF = 0;
    double TIME_TAKEOFF = 3;

    ros::Time plan_start_time;

    bool at_end{false};
    bool rec_global_goal_flag = true; // 是否收到了全局目标点
    double replan_time = 1.0;

    GridMapNode tmp_node1;
    GridMapNode tmp_node2;

    bool at_goal = false;

    int sendpva_count_start{0};

    bool gvmInitial{false};

    double MIN_GOAL_DIS{0.3};
    double REPLAN_DIST{0.3};
    double EXEC_TIME{0.1};
    bool SET_POINT_CONTROL{false};
    bool NEED_OFFBOARD_READY{false};
    double MAP_VISUAL_TIME{0.1};

    int visual_map_range{5};

    bool VISUAL_TRANS_CLOUD{true};

    bool ENTER_EXEC_CALLBACK{true};

    bool SAVE_TRAJ{true};

    std::ofstream outFile;

    double init_time;
    double traj_start_time;

    double plan_traj_use_time{0.035};
    bool NEED_GLOBAL_POS;

    ros::Publisher plan_time_pub;

    // 获取当前功能包的路径
    std::string packagePath;

};


#endif